#include <libpub.h>
#include <bo_cmds.h>

extern "C" {
int isgraph(int c);
}

//===================
//      Memory Operations
//===================
extern "C" {
int Do_Memcmp(char *param);
}
#define CMD_MEMCMP {Do_Memcmp, \
    "comp --compare two memory area",    \
    "Help:  memory comp add1 addr2 len\n" \
    "Examples: memory comp 0xa1c00000 0xa0c00000 10\n"\
     "len maybe Hex or Dec.Default is Hex.\".10\"means Dec 10\n"\
}

extern "C" {
int Do_Memcpy(char *param);
}
#define CMD_MEMCPY {Do_Memcpy, \
    "copy  --copy  memory area",    \
    "Help:  memory copy src dst len\n" \
   "memory copy 0xa1c00000 0xa0c00000 10\n"\
    "len maybe Hex or Dec.Default is Hex.\".10\"means Dec 10\n"}

 extern "C" {
int Do_Memset(char *param);
}
#define CMD_MEMSET {Do_Memset, \
    "fill --set  memory area",    \
    "Help:  memory fill addr ch len\n" \
    "memory fill 0xa1c00000 a 10"\
   "len maybe Hex or Dec.Default is Hex.\".10\"means Dec 10\n"}

 extern "C" {
int Do_Memdump(char *param);
}
#define CMD_MEMDUMP {Do_Memdump, \
    "dump --memory dump addr len",    \
    "Help:  dump memory area\n"\
    "memory dump 0xa0c00000 10" \
    "len maybe Hex or Dec.Default is Hex.\".10\"means Dec 10\n"\
   }

 extern "C" {
int Do_MemdumpB(char *param);
}
#define CMD_MEMDUMPB {Do_MemdumpB, \
    "dumpb --memory dump addr len",    \
    "Help:  dump memory by bytes\n"\
    "memory dumpb 0xa0c00000 10" \
    "len maybe Hex or Dec.Default is Hex.\".10\"means Dec 10\n"\
}


extern "C" {
int Do_Memmove(char *param);
}
#define CMD_MEMMOVE {Do_Memmove, \
    "move --move  memory area",    \
    "Help:  memory move  src  dst  len\n"\
    " " \
   "\n"}

tCommandTbl Bo_MemoryCmdTbl[] = {
    CMD_MEMCMP,
    CMD_MEMCPY,
    CMD_MEMSET,
    CMD_MEMDUMP,
    CMD_MEMMOVE,
    CMD_MEMDUMPB,
    CMD_NULL,               //DO NOT REMOVE
};

int Do_Memory(char *param)
{
    if (!*param)  return 0;
    return Bo_ExeCommand(Bo_MemoryCmdTbl, param);
}

int Do_Memcmp(char *param)
{
    UINT32 equal;
    UINT32 i;
    UINT32 address1,  address2,  length;
    int no = 0;
    int same = 0;

    equal=1;

    if (*param) {
        param = Bo_ParseInteger(param, (unsigned long *) &address1);
        if (*param) {
            param = Bo_ParseInteger(param, (unsigned long *) &address2);
            if (*param) {
                param = Bo_ParseInteger(param, (unsigned long *) &length);
                goto cmpstart;
            }
        }
    }

    return -1;

cmpstart:
    if(length>0){
        for(i = 0;i < length; i++){
            if((*(char *) (address1 + i)) != (*(char *)(address2 + i))){
                printf("%6d)  0X%x : %02x   <-->   0X%x : %02x\n", ++no, address1 + i, *((char *)(address1 + i)), address2 + i, *((char *)(address2 + i)));
                equal = 0;
            }
            else
                same++;
        }
        if(equal == 1)
            printf("ALL Equal data--Total:%6d\n", same);
        else
            printf("\nsame data total=%6d", same);
    }
    return 0;
}

int Do_Memcpy(char *param)
{
    register UINT32 i, cnt, nbytes;
    UINT32 src,  dst,  length;

    if (*param) {
         param = Bo_ParseInteger(param, (unsigned long *) &src);
         if (*param) {
             param = Bo_ParseInteger(param, (unsigned long *) &dst);
             if (*param) {
                 param = Bo_ParseInteger(param, (unsigned long *) &length);
                 goto memcpystart;
             }
         }
     }
    printf("\n Error: param is null!\n");
    return -1;

memcpystart:
    if(length > 0){
        cnt = length /sizeof(unsigned long);
        nbytes = sizeof(unsigned long);

         for(i = 0; i < cnt; i++)
                *((unsigned long *)(dst + i * nbytes)) = (*(unsigned long *)(src + i * nbytes));

        switch (length % 4) { //Handle Rest Bytes
        case 1:
            *((char *)(dst + i * nbytes)) = (*(char *)(src + i * nbytes));
            break;

        case 2:
            *((unsigned short *)(dst + i * nbytes)) = (*(unsigned short *)(src + i * nbytes));
            break;

        case 3:
            *((unsigned short *)(dst + i * nbytes)) = (*(unsigned short *)(src + i * nbytes));
            *((char *)(dst + i * nbytes + sizeof(unsigned short))) = (*(char *)(src + i * nbytes + sizeof(unsigned short)));
            break;

        default:
            break;
      }
    }
    else{
        printf("\nCommad Parameter Error:length <=0\n",length);
        return -1;
    }
    return 0;
}

int Do_Memset(char *param)
{
    register UINT32 i;
    UINT32 addr,  length, _ch;
    char ch;

   length = 0;
    if (*param) {
         param = Bo_ParseInteger(param, (unsigned long *) &addr);
         if (*param) {
             param = Bo_ParseInteger(param, (unsigned long *) &_ch);
             if (*param) {
                 param = Bo_ParseInteger(param, (unsigned long *) &length);
                 goto memsetstart;
             }
         }
     }
    printf("\n Error: param is null!\n");
    return -1;

memsetstart:
    ch = (char) _ch;
    if(length>0){
        for(i = 0; i < length; i++)
            *((char *)(addr + i)) = ch;
    }
    else{
        printf("\nCommad Parameter Error:length <=0\n", length);
        return -1;
    }
    return 0;
}


int Do_Memdump(char *param)
{
    register UINT32 i;
    UINT32 addr,  length;

    if (*param) {
           param = Bo_ParseInteger(param, (unsigned long *) &addr);
           if (*param) {
               param = Bo_ParseInteger(param, (unsigned long *) &length);
               goto memdumpstart;
           }
   }
   printf("\n Error: param is null!\n");
   return -1;

memdumpstart:
    if(length>0){
        for(i=0; i<= (length -1)/16; i++){
            printf("0x%08x : %08x %08x %08x %08x\n", addr, *((UINT32*)(addr)), *((UINT32*)(addr + 4)), *((UINT32*)(addr + 8)), *((UINT32*)(addr + 12)));
            addr += 16;
        }
    }
    else{
        printf("\nCommad Parameter Error:length <=0\n", length);
        return -1;
    }

    return 0;
}


static inline INT32 PrintAddr(UINT32 addr, UINT32 i, UINT8 width)
{
    printf("\n%08Xh:", addr + i * width );
    return 0;
}

static inline UINT8 PrintByte(UINT32 addr, UINT32 i, UINT32 j, UINT8 width)
{
    UINT8 ch = 0;

    ch = *((UINT8 *) (addr + i * width + j));
    printf("%02x ", ch);
    return ch;
}

static inline INT32 PrintAscii(UINT8 *ch_array, INT32 len)
{
    register INT32 i;

    if(!ch_array || len < 0)
        return -1;

    printf(";");
    for(i = 0; i < len; i++){
        if(!!isgraph(ch_array[i])){
            printf("%c",ch_array[i]);
        }
        else
            printf(" ");
    }

    return 0;
}

static inline INT32 PrintRestBlank(UINT8 len)
{
    register INT32 i;
    UINT8 _len = 3 * len;
    for(i = 0; i < _len ; i++)
        printf(" ");
    return 0;
}

static inline UINT8 ConvertToASCII(UINT8 ch)
{
    if(/*ch >= 0x00 &&*/ ch <= 0x7F)
        return ch;
    else
        return '.';
}

int Do_MemdumpB(char *param)
{
    register UINT32 i;
    UINT32 addr, printed_line;
    UINT32 length;
    const UINT8 width = 16;
    UINT8 j = 0;
    UINT8 rest = 0, ch = 0, ch_array[width];

    if (*param) {
           param = Bo_ParseInteger(param, (unsigned long *) &addr);
           if (*param) {
               param = Bo_ParseInteger(param, (unsigned long *) &length);
               goto memdumpbstart;
           }
   }
   printf("\n Error: param is null!\n");
   return -1;

memdumpbstart:
    if(length>0){
        for(i = 0; i< length / width; i ++){
            PrintAddr(addr, i, width);
            for (j = 0; j < width; j++){
                 ch = PrintByte(addr, i, j, width);
                 ch_array[j] = ConvertToASCII(ch);
             }
            PrintAscii(ch_array, width);
        }
 //Handle The Last Part Of The Rest
        rest = length % width;
        printed_line = i;
        if(rest != 0){
            PrintAddr(addr, printed_line, width);
            for(j = 0; j < rest; j++ ){
                ch = PrintByte(addr, printed_line, j, width);
                ch_array[j] = ConvertToASCII(ch);
            }
            PrintRestBlank(width - rest);
            PrintAscii(ch_array, rest);
        }
        printf("\n");
    }
    else{
        printf("\nCommad Parameter Error:length <=0\n", length);
        return -1;
    }

    return 0;
}

int Do_Memmove(char *param)
{
    register UINT32 i;
    UINT32 src,  dst,  length, _length;

    if (*param) {
         param = Bo_ParseInteger(param, (unsigned long *) &src);
         if (*param) {
             param = Bo_ParseInteger(param, (unsigned long *) &dst);
             if (*param) {
                 param = Bo_ParseInteger(param, (unsigned long *) &length);
                 goto memmovestart;
             }
         }
     }
    printf("\n Error: param is null!\n");
    return -1;

memmovestart:
//===========
    if(length > 0){
        if(src + length <= dst || dst < src - length){//No Overlap
           for(i=0; i<length; i++)
               *((char *)(dst + i)) = (*(char *)(src + i));
           for(i=0; i<length; i++)
                *((char *)(src + i)) = 0;
       }
       else if(dst < src + length){//Handle Upper Overlap
           for(i = length; i > 0; i--)
                   *((char *)(dst + i - 1)) = (*(char *)(src + i - 1));
            _length = dst - src;
           for(i = 0; i < _length; i++)
                *((char *)(src + i)) = 0;
       }
       else{ //Handle Lower Overlap
           for(i = 0; i < length; i++)
               *((char *)(dst + i)) = (*(char *)(src + i));
            _length = src - dst;
           for(i = 0; i < _length; i++)
                *((char *)(dst + length + i)) = 0;
      }// if(length > 0)
    }
   else{
        printf("\nCommad Parameter Error:length <= 0\n", length);
        return -1;
    }
   return 0;
}
